#include <omplapp/apps/SE2RigidBodyPlanning.h>
#include <omplapp/config.h>
#include "RRTIterationCount.h"
#include "pRRTIterationCount.h"
#include <ompl/base/samplers/UniformValidStateSampler.h>
#include "PoissonRRT.h"
#include "PoissonSampler.h"

using namespace ompl;

base::ValidStateSamplerPtr allocUniformStateSampler(const base::SpaceInformation *si)
{
	return base::ValidStateSamplerPtr(new base::UniformValidStateSampler(si));
}

// graph draw
int main()
{
	// plan in SE2
	app::SE2RigidBodyPlanning setup;

	// load the robot and the environment
	std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/car1_planar_robot.dae";
	std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/RandomPolygons_planar_env.dae";
	setup.setRobotMesh(robot_fname.c_str());
	setup.setEnvironmentMesh(env_fname.c_str());

	// define starting state
	base::ScopedState<base::SE2StateSpace> start(setup.getSpaceInformation());
	start->setX(-45);
	start->setY(-45);
	start->setYaw(0.0);

	// define goal state
	base::ScopedState<base::SE2StateSpace> goal(start);
	goal->setX(45);
	goal->setY(45);
	goal->setYaw(0.0);

	// set the start & goal states
	setup.setStartAndGoalStates(start, goal);

	// set Poisson-disk sampler
	//setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
	setup.getSpaceInformation()->setValidStateSamplerAllocator(poisson::PoissonSampler::allocPoissonStateSampler);

	// set Poisson-disk planner
	//setup.setPlanner(base::PlannerPtr(new RRTIterationCount(setup.getSpaceInformation())));
	//setup.setPlanner(base::PlannerPtr(new pRRTIterationCount(setup.getSpaceInformation(), 8)));
	setup.setPlanner(base::PlannerPtr(new poisson::PoissonRRT(setup.getSpaceInformation()))); // todo: set parallel=8

	//ompl::base::StateSpacePtr stateSpace = setup.getStateSpace();
	//base::RealVectorBounds bounds = setup.getStateSpace()->as<base::SE2StateSpace>()->getBounds();

	//std::vector<ompl::base::StateSpacePtr> subspaces = setup.getStateSpace()->as<base::SE2StateSpace>()->getSubspaces();

	// attempt to solve the problem, and print it to screen if a solution is found
	if (setup.solve(30.0))
	  setup.getSolutionPath().print(std::cout);

	return 0;
}
